/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-10-13     Administrator       the first version
 */
#include <Motor.h>

//rt_uint32_t period,   周期
//rt_uint32_t pulse,    脉冲
//rt_uint8_t SIGN,      转向：PIN_LOW 顺时针  PIN_HIGH 逆时针
//FunctionalState state 电机是否开启，ENABLE 开启
void Motor_Start(rt_uint32_t period,rt_uint32_t pulse, rt_uint8_t SIGN, FunctionalState state)
{
    //SIGN--DIR PB10 正反转
    rt_pin_mode(SIGN_PIN, PIN_MODE_OUTPUT);
    rt_pin_write(SIGN_PIN, SIGN);
    //使能引脚
    rt_pin_write(GET_PIN(B,12) , PIN_HIGH);
    rt_pin_write(GET_PIN(B,13) , PIN_LOW);
    rt_thread_mdelay(1);

    //pwm输出  PA8
//    PWM1_Init();
    PWM1_CH1_Set(period, pulse, state);
}

//电机停止
void Motor_Stop(void)
{
    rt_pin_write(GET_PIN(B,12) , PIN_LOW);
    rt_pin_write(GET_PIN(B,13) , PIN_HIGH);
    PWM1_CLOSE();
}

//步骤标志
rt_uint8_t Step = 1;
//停止标志
rt_uint8_t StopStep = 0;

//检测灵敏度
float Sensitivity = 0.2;

//step1 启动电机
void MotorStart(LoadDataFrm pLoad, rt_uint32_t period, rt_uint32_t pulse)
{
    if (Step == 1)
    {

        if (pLoad.cur_realforce > Sensitivity)
        {
            rt_pin_write(GET_PIN(B,12) , PIN_HIGH);
            Motor_Start(period, pulse, PIN_LOW, ENABLE);
            Step = 2;
            StopStep = StepFalling;
        }
        else if (pLoad.cur_realforce < -Sensitivity ) {
            rt_pin_write(GET_PIN(B,12) , PIN_HIGH);
            Motor_Start(period, pulse, PIN_HIGH, ENABLE);
            Step = 2;
            StopStep = StepRasing;
        }
    }
}

//step2 停止电机
void MotorStop(LoadDataFrm pLoad, rt_uint32_t period, rt_uint32_t pulse)
{
    //自动模式变速启动
//    AutomaticMode();

    if (Step == 2)
    {
        if (StopStep == StepFalling)
        {
            if (pLoad.cur_realforce < Sensitivity-0.05)
            {
                rt_pin_write(GET_PIN(B,12) , PIN_LOW);
                Motor_Stop();
                Step = 1;
                StopStep = 0;
            }
        }
        else if (StopStep == StepRasing)
        {
            if (pLoad.cur_realforce > -Sensitivity+0.05)
            {
                rt_pin_write(GET_PIN(B,12) , PIN_LOW);
                Motor_Stop();
                Step = 1;
                StopStep = 0;
            }
        }
    }
}
MotorData_t StartData;
void AutomaticMode (void)
{
    //进入自动模式
    if (KeyFlag.key_up_data == 3) {
        //电机启动参数 随力值变化

        //y=-175000*x+285000
        //period = y
        //pulse = (rt_uint32_t)y/2
        if (pLoad.cur_realforce > 1.2 || pLoad.cur_realforce < -1.2) {
            StartData.period = 300000/4;
            StartData.pulse = 300000/8;
        }else {
            StartData.period = -175000*pLoad.cur_realforce*2+285000;
            StartData.pulse = (rt_uint32_t) StartData.period / 2;
        }
        //启动
        if (pLoad.cur_realforce > 0) {
            Motor_Start(StartData.period, StartData.pulse, PIN_LOW, ENABLE);
        }else {
            Motor_Start(StartData.period, StartData.pulse, PIN_HIGH, ENABLE);
        }
    }
}


void MotionDetection (LoadDataFrm pLoad, rt_uint32_t period, rt_uint32_t pulse)
{
//    check(pLoad);
    MotorStart(pLoad, period, pulse);
    MotorStop(pLoad, period, pulse);
}

//检测状态复位
void DetectionStatusReset(void)
{
    //恢复启动状态
    Step = 1;
    //上升下降初始值复位
    StopStep = 0;
}









